import redis
import json
import pickle
import numpy as np
import time

r = redis.Redis(db=0)

gps_vio_list = []

if __name__ == '__main__':
    r.set('vio_switch', 'off')
    time.sleep(1)
    r.set('vio_switch', 'on')

    while r.get('process_5').decode("utf-8") == 'on':
        current_gps = json.loads(r.lindex('gps_queue', 0))

        found = False
        for vio in r.lrange('vio_queue', 0, 1000):
            vio_data = json.loads(vio)
            if 0.1 < current_gps[5] - vio_data[3]:
                current_data = current_gps + vio_data
                found = True
                break

        if len(gps_vio_list) == 0:
            if found:
                gps_vio_list = [current_data] + gps_vio_list
        elif current_gps[5] == gps_vio_list[0][5]:
            pass
        elif gps_vio_list[0][6] - 0.01 < current_data[6] < gps_vio_list[0][6] + 0.01 \
                and gps_vio_list[0][7] - 0.01 < current_data[7] < gps_vio_list[0][7] + 0.01\
                and gps_vio_list[0][8] - 0.01 < current_data[8] < gps_vio_list[0][8] + 0.01:
            if current_data[3] <= gps_vio_list[0][3]:
                gps_vio_list[0] = current_data
        else:
            gps_vio_list = [current_data] + gps_vio_list

        print(len(gps_vio_list))

        gps_vio_list = gps_vio_list[:100]

        if current_gps[3] < 100:
            r.set('process_5_log', 'gps accurate')

            r.set('fusion_lat', current_gps[0])
            r.set('fusion_lon', current_gps[1])
            r.set('gps_timestamp', time.time())
        else:
            if len(gps_vio_list) > 30:
                print('fusion')
                processing_list = gps_vio_list[:50]
                last_item = processing_list[-1]

                gps_cor = np.array(
                    [[(x[0] - last_item[0]) * 110867.3139703155] + [(x[1] - last_item[1]) * 95628.01014437043] + [
                        (x[2] - last_item[2]) / 1000] for x in processing_list])

                vio_cor = np.array(
                    [[x[6] - last_item[6]] + [x[7] - last_item[7]] + [x[8] - last_item[8]] + [1] for x in processing_list])

                w = np.array([x[3]*2 for x in processing_list])

                gw = gps_cor / np.sqrt(w[:, np.newaxis])
                vw = vio_cor / np.sqrt(w[:, np.newaxis])

                X = np.linalg.lstsq(vw, gw)

                gps_vio_cor = np.array(np.mat(vio_cor[0]) * np.mat(X[0]))
                result = [gps_vio_cor[0][0]/110867.3139703155 + last_item[0], gps_vio_cor[0][1]/95628.01014437043 + last_item[1]]

                r.set('fusion_lat', result[0])
                r.set('fusion_lon', result[1])
                r.set('gps_timestamp', time.time())
            else:
                r.set('fusion_lat', current_gps[0])
                r.set('fusion_lon', current_gps[1])
                r.set('gps_timestamp', time.time())
        # with open('gps_vio_list.pickle', 'wb') as f:
        #     pickle.dump(gps_vio_list, f)
